package source.calcCmd;

import source.Enums.Direction;
import source.Enums.Side;
import source.control.Command;

public class RobotCommand {

	private Direction dirL, dirR;
	
	private int speedL, speedR;
	private int stepsL, stepsR;
	
	private int realTime;
	private double realVL, realVR;
	
	private int prevSpeedL,prevSpeedR;
	
	
	public int getRealTime(){
		return realTime;
	}
	
	public int getPrevSpeed(Side side){
		switch (side){
		
		case LEFT:
			return prevSpeedL;
		case RIGHT:
			return prevSpeedR;
		default:
			return 0;
		
		}
	}
	
	public double getRealV(Side side){
		switch (side){
		
		case LEFT:
			return realVL;
		case RIGHT:
			return realVR;
		default:
			return 0;
		
		}
	}
	
	public RobotCommand(Direction dirL, Direction dirR, int speedL, int speedR, int stepsL, int stepsR){
		this.dirL = dirL;
		this.dirR = dirR;
		this.speedL = speedL;
		this.speedR = speedR;
		this.stepsL = stepsL;
		this.stepsR = stepsR;
	}
	
	public RobotCommand(RobotCommand cmd){
		this.dirL = cmd.dirL;
		this.dirR = cmd.dirR;
		this.speedL = cmd.speedL;
		this.speedR = cmd.speedR;
		this.stepsL = cmd.stepsL;
		this.stepsR = cmd.stepsR;
		this.prevSpeedL = cmd.prevSpeedL;
		this.prevSpeedR = cmd.prevSpeedR;
		
		this.realTime = cmd.realTime;
		this.realVL = cmd.realVL;
		this.realVR = cmd.realVR;
	}
	
	public void newCommand(double vL, double vR){
		this.newCommand(vL, vR, realTime, prevSpeedL, prevSpeedR);
	}
	
	public RobotCommand(Command cmd, int prevSpeedL, int prevSpeedR){
		newCommand(cmd,prevSpeedL,prevSpeedR);
	}
	
	public void newCommand(Command cmd,int prevSpeedL,int prevSpeedR){
		newCommand(	cmd.getV(Side.LEFT),
					cmd.getV(Side.RIGHT),
					cmd.getTime(),
					prevSpeedL,
					prevSpeedR );
	}
	
	public void newCommand(double vL, double vR, int tMs, int prevSpeedL, int prevSpeedR){

		this.prevSpeedL = prevSpeedL;
		this.prevSpeedR = prevSpeedR;
		
		this.dirL = Direction.FORWARD;
		this.dirR = Direction.FORWARD;
		if (vR<0) { this.dirR = Direction.BACKWARD; vR = -vR; }
		if (vL<0) { this.dirL = Direction.BACKWARD; vL = -vL; }
		
		double stepDistanceM = (RealLifeProperties.StepDistanceMM/1000);
		double timeS = tMs/1000.0;

		// calculates the required parameter to give to the robot's control
		this.speedL = RealLifeProperties.vToSpeed(vL);
		this.speedR = RealLifeProperties.vToSpeed(vR);

		double tAccL = getAccTime(speedL,prevSpeedL);
		double tAccR = getAccTime(speedR,prevSpeedR);
		
		if (tAccL <= timeS ){
			this.stepsL = (int) Math.round((timeS-tAccL)*RealLifeProperties.timerFrequency/(speedL*2)) + Math.abs(prevSpeedL-speedL);
		} else {
			int dir = 1;
			if (speedL > prevSpeedL){
				dir = -1;
			}
			
			while (tAccL > timeS){
				speedL += dir;
				tAccL = getAccTime(speedL,prevSpeedL);
			}
			
			this.stepsL = (int) Math.round((timeS-tAccL)*RealLifeProperties.timerFrequency/(speedL*2)) + Math.abs(prevSpeedL-speedL);
			//speedL -= dir;
		}
		
		if (tAccR <= timeS){
			this.stepsR = (int) Math.round((timeS-tAccR)*RealLifeProperties.timerFrequency/(speedR*2)) + Math.abs(prevSpeedR-speedR);
		} else {
			int dir = 1;
			if (speedR > prevSpeedR){
				dir = -1;
			}
			
			while (tAccR > timeS){
				speedR += dir;
				tAccR = getAccTime(speedR,prevSpeedR);
			}
			
			this.stepsR = (int) Math.round((timeS-tAccR)*RealLifeProperties.timerFrequency/(speedR*2)) + Math.abs(prevSpeedR-speedR);
			//speedR -= dir;
		}
		
		// calculates the required number of steps
		//this.stepsL = (int) Math.round(vL*(timeS-accL)/stepDistanceM) + Math.abs(speedL-prevSpeedL)*2;
		//this.stepsR = (int) Math.round(vR*(timeS-accR)/stepDistanceM) + Math.abs(speedR-prevSpeedR)*2;
		
		
		
		if (stepsL==0) speedL = 0;
		if (stepsR==0) speedR = 0;
		
		if (vL == 0){
			stepsL = RealLifeProperties.timerMaxValue-prevSpeedL;
			speedL = RealLifeProperties.timerMaxValue;
		}
		if (vR == 0){
			stepsR = RealLifeProperties.timerMaxValue-prevSpeedR;
			speedR = RealLifeProperties.timerMaxValue;
		}
		
		realTime = tMs;
		realVR = vR;
		realVL = vL;
		
		if (dirL == Direction.BACKWARD){
			realVL *= -1;
		}
		if (dirR == Direction.BACKWARD){
			realVR *= -1;
		}

	}
	
	public String toString(){
		String out = "";
		// 1 is Right!
		String dir1 = "f";
		String dir2 = "f";
		
		if (dirR == Direction.BACKWARD){
			dir1 = "b";
		}
		if (dirL == Direction.BACKWARD){
			dir2 = "b";
		}
		
		String sp1 = intToString(speedR,3);
		String sp2 = intToString(speedL,3);
		String st1 = intToString(stepsR,4);
		String st2 = intToString(stepsL,4);
		
		out = "run " + dir1 + dir2 + " " +sp1+" "+sp2+" "+st1+" "+st2;
		
		return out;
	}
	
	public RobotCommand(double vL, double vR, int tMs, int prevSpeedL, int prevSpeedR){
		this.newCommand(vL,vR,tMs,prevSpeedL,prevSpeedR);
	}
	
	public static RobotCommand toZero(int prevSpeedL, int prevSpeedR, Direction dirL, Direction dirR){
		
		int stTo0L = RealLifeProperties.timerMaxValue-prevSpeedL;
		int stTo0R = RealLifeProperties.timerMaxValue-prevSpeedR;
		
		RobotCommand ret = new RobotCommand(dirL, dirR, RealLifeProperties.timerMaxValue, RealLifeProperties.timerMaxValue, stTo0L, stTo0R);
		
		return ret;
	}
	
	public void changeDir(Side s, Direction d){
		
		int dir;
		
		switch (s){
		case LEFT:
			
			dir = (d == Direction.FORWARD ? 1 : -1);
			dirL = d;
			
			realVL = dir * Math.abs(realVL);
			break;
		case RIGHT:
			
			dir = (d == Direction.FORWARD ? 1 : -1);
			dirR = d;
			
			realVR = dir * Math.abs(realVR);
			break;
		}
	}
	
	private double calcCmdTime(int speed, int prevSpeed, int steps){
		double time = 0;
		
		int diff = speed>prevSpeed ? speed-prevSpeed : prevSpeed-speed;
		
		time += (speed*(speed+1)/2.0)-(prevSpeed*(prevSpeed)/2.0);
		
		if (speed < prevSpeed){
			time *= -1;
		}
		
		time += (steps-diff-1)*speed;
		
		time /= (double)RealLifeProperties.timerFrequency;
		
		return time;
	}
	
	public double getAccTime(int sp, int prevSp){
		double accL = (sp*(sp+1)/2.0)-(prevSp*(prevSp+1)/2.0);
		accL *= 2;
		
		if (sp < prevSp){
			accL *= -1;
		}
	
		accL /= RealLifeProperties.timerFrequency;
		return accL;
	}
	
	public RobotCommand(String cmd){
		if (cmd.charAt(4) == 'f'){
			dirL = Direction.FORWARD;
		} else {
			dirL = Direction.BACKWARD;
		}
		
		if (cmd.charAt(5) == 'f'){
			dirR = Direction.FORWARD;
		} else {
			dirR = Direction.BACKWARD;
		}
		
		speedL = Integer.parseInt(cmd.substring( 7, 10));
		speedR = Integer.parseInt(cmd.substring(11, 13));
		
		stepsL = Integer.parseInt(cmd.substring(14, 18));
		stepsR = Integer.parseInt(cmd.substring(19, 23));
		
	}
	
	public Direction getDirL(){
		return dirL;
	}
	
	public Direction getDirR() {
		return dirR;
	}

	public int getSpeedL() {
		return speedL;
	}

	public int getSpeedR() {
		return speedR;
	}

	public int getStepsL() {
		return stepsL;
	}

	public int getStepsR() {
		return stepsR;
	}

	public static String intToString(int i, int len){
		String ret = "";
		if(i<10){
			for(int x=0;x<len-1;x++)
				ret += '0';
			ret += String.valueOf(i);
		}
		else if(i<100){
			for(int x=0;x<len-2;x++)
				ret += '0';
			ret += String.valueOf(i);
		}
		else if(i<1000){
			for(int x=0;x<len-3;x++)
				ret += '0';
			ret += String.valueOf(i);
		}
		else
			ret = String.valueOf(i);
		return ret;
	}
	
}
